﻿#include "interpolationCurveDeri.h"
#include <QFileDialog>
#include <fstream>
#include <QGLWidget>
#include <gl/glu.h>


/**************************************************
@brief   : 实现曲线的差值 添加了两条端点向量
@author  : lee
@step    : 1. json文件读取需要插值的控制点
		   2. 调用算法实现样条曲线
		   3. 绘制样条曲线
@output  : none
@time    : none
**************************************************/

/**************************************************
@brief   : InterpolateCurve构造函数
@author  : lee
@input   : none
@output  : none
@time    : none
**************************************************/
InterpolateCurveDeri::InterpolateCurveDeri(QWidget *parent) :
	Strategy(), QGLWidget(parent) {
	flag = -1;// 没有任何的鼠标事件
	rotationX = 0;
	rotationY = 0;
	rotationZ = 0;
	p = 0;
	curvePointNum = 0;
	times = 1.0;
};



/**************************************************
@brief   : 析构函数释放相关资源
@author  : lee
@input   : none
@output  : none
@time    : none
**************************************************/
InterpolateCurveDeri::~InterpolateCurveDeri() {}

/**************************************************
@brief   : 全局曲线插值	ALGORITHM A9.1 + 增加两个向量的方程
@author  : lee
@input   : Q 要拟合的点
		   n // n+1个要拟合的点
		   r 坐标的维度
		   p 拟合的曲线的阶次
@output  : m // U_0 - U_m 中的m
		   U //
		   P // ctrlPoints
@time    : none
**************************************************/
void InterpolateCurveDeri::globalCurveInterp(const std::vector<Point3d> &Q,
	int p, int &m, std::vector<double> &U, std::vector<Point3d> &ctlPts) {
	int n = Q.size() - 1;
	m = n + p + 3;// 个人觉得没必要增加节点和节点向量觉得书上写的不一定对的， 还是使用了单纯增加两条方程而控制点不增加
	// 计算u_k 通过 Eq.9.5
	double sumChordLength = 0;
	for (int i = 1; i < n + 1; i++) {
		sumChordLength += chordLength(Q[i], Q[i - 1]);
	}
	std::vector<double> u_k;// Q点对应的u
	u_k.resize(Q.size());
	u_k[0] = 0.0;
	for (int i = 1; i < n + 1; i++) {
		u_k[i] = u_k[i - 1] + chordLength(Q[i], Q[i - 1]) / sumChordLength;
	}//插值点对应的u_k???   然后形成U？？  Eq.(9.5)
	// 计算U 通过 Eq. 9.8
	U.resize(m + 1);
	for (int i = 0; i <= p; i++) {
		U[i] = 0;
	}
	for (int j = 0; j <= n - p + 1; j++) {
		double tmp = 0;
		for (int i = j; i <= j + p - 1; i++) {
			tmp += u_k[i];
		}
		U[j + p + 1] = 1.0 / p * tmp;
	}
	for (int i = m - p; i <= m; i++) {
		U[i] = 1;
	}
	Eigen::MatrixXd N = Eigen::MatrixXd::Zero(n + 3, n + 3);//增加两条方程

	for (int i = 0; i <= n; i++) {
		// 构造N矩阵
		int span = findSpan(n+2, p, u_k[i], U);
		std::vector<double> base;
		basisFuns(span, u_k[i], p, U, base);// N[i][span - p]
		for (int j = 0; j < base.size(); j++) {
			N(i, span - p + j) = base[j];
		}
	}
	N(n + 1, 0) = -1;
	N(n + 1, 1) = 1;
	N(n + 2, n + 1) = -1;
	N(n + 2, n+2) = 1;
	// test N
	/*for (int i = 0; i < n + 1; i++) {
		for (int j = 0; j < n + 1; j++) {
			std::cout << N(i, j) << " ";
		}
		std::cout << std::endl;
	}*/
	// LUDecomposition(A,n+1,p-1):  LU分解  NP=F  求P
	Eigen::MatrixXd F = Eigen::MatrixXd::Zero(n + 3, 3);// 增加两个结果
	for (int i = 0; i < n + 1; ++i)
	{
		F(i, 0) = Q[i].x;
		F(i, 1) = Q[i].y;
		F(i, 2) = Q[i].z;
	}
	F(n + 1, 0) = (U[p + 1]) / p * D0(0);
	F(n + 1, 1) = (U[p + 1]) / p * D0(1);
	F(n + 1, 2) = (U[p + 1]) / p * D0(2);
	F(n + 2, 0) = (1 - U[m-p-1]) / p * Dn(0);
	F(n + 2, 1) = (1 - U[m-p-1]) / p * Dn(1);
	F(n + 2, 2) = (1 - U[m - p - 1]) / p * Dn(2);
	Eigen::MatrixXd P(n + 3, 3);
	P = N.fullPivLu().solve(F);
#ifdef _DEBUG
	std::cout << "N--------------" << std::endl << N << std::endl;
	std::cout << "F--------------" << std::endl << F << std::endl;
	std::cout << "P--------------" << std::endl << P << std::endl;
#endif
	ctlPts.clear();
	for (int i = 0; i < n + 3; i++) {
		Point3d point(P(i, 0), P(i, 1), P(i, 2));
		ctlPts.push_back(point);
	}
}


/**************************************************
@brief   : 计算弦长 chord length 单纯两点之间的距离？？
@author  : lee
@input   : vecFitPoints 要拟合的点
@output  : none
@time    : none
**************************************************/
double InterpolateCurveDeri::chordLength(const Point3d & a, const Point3d &b) {
	double length = sqrt((a.x - b.x)*(a.x - b.x) +
		(a.y - b.y)*(a.y - b.y) + (a.z - b.z)*(a.z - b.z));
	return length;
}


/**************************************************
@brief   : 找到 u 所属于的区间序列
@author  : lee
@input   : n n = m - p -1
		   p 基函数的阶
		   u 递增的一般从0到1上的点
		   U 节点向量 U_0 - U_m
@output  : 节点区间序列
@time    : none
**************************************************/
int InterpolateCurveDeri::findSpan(int n, int p, double u, const std::vector<double> &U) {
	if (u >= U[n + 1]) return n;
	for (int i = p; i < n + 1; i++) {
		if (U[i] <= u && u < U[i + 1]) {
			return i;
		}
	}
}

/**************************************************
@brief   : 计算非空的基函数
		   第一次，p=0时，只有N[0]=1.0,即N0,0=1.0;p=1时，求N[0],N[1],即N0,1和N1,1;p=2时，求N[0],N[1],N[2],即N0,2  N1,2和N2,2
@author  : lee
@input   ：i 节点向量的第i个序列
		   u 生成曲线上的点
		   order 阶次
		   U 节点向量
@output  ：base 从第0阶到后面的阶辐射的三角形所对应的阶函数的值，一整套
@time    : none
**************************************************/
void InterpolateCurveDeri::basisFuns(int i, double u, int order, const std::vector<double> &U, std::vector<double> &base) {
	base.resize(order + 1);
	base[0] = 1.0;
	std::vector<double> left, right;
	left.resize(order + 1);
	right.resize(order + 1);
	for (int j = 1; j <= order; j++) {
		left[j] = u - U[i + 1 - j];
		right[j] = U[i + j] - u;
		double saved = 0.0;
		for (int r = 0; r < j; r++) {
			double temp = base[r] / (right[r + 1] + left[j - r]);
			base[r] = saved + right[r + 1] * temp;
			saved = left[j - r] * temp;
		}
		base[j] = saved;
	}
}


/**************************************************
@brief   : 生成曲线上的点
		   关键公式  m = n + p + 1
@author  : lee
@input   : n+1 控制点的个数
		   p 阶次
		   m+1 节点的个数
		   P  控制点数组
		   u 从小到大，从而绘制出整个曲线
		   C 生成的点的坐标
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::curvePoint(int n, int p, const std::vector<double>&U, std::vector<Point3d>&P, double u, Point3d &C) {
	int span = findSpan(n, p, u, U);
	std::vector<double> base;
	basisFuns(span, u, p, U, base);
	double x, y, z;
	x = y = z = 0;
	for (int i = 0; i <= p; i++) {
		x = x + P[span - p + i].x * base[i];
		y = y + base[i] * P[span - p + i].y;
		z = z + base[i] * P[span - p + i].z;
	}
	Point3d temp;
	temp.x = x;
	temp.y = y;
	temp.z = z;
	C = temp;
}


/**************************************************
@brief   : 简单的讲生成的点加入到曲线上
@author  : lee
@input   ：str  文件路径
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::genPoints(std::string str) {
	jsonReader(str);
	std::cout << "[DEBUG]genPoint" << std::endl;
	int m;
	std::vector<double> U;

	globalCurveInterp(Q, p, m, U, ctlPts);
	BSplineCurveAbstract bs(p, ctlPts.size() - 1, U, ctlPts);

	for (int i = 0; i <= curvePointNum; i++) {
		double u = i * 1.0 * (U[U.size() - 1] - U[0]) / curvePointNum;//均匀分成多少个点再所有的节点上
		Point3d C;
		curvePoint(ctlPts.size() - 1, p, U, ctlPts, u, C);
		curvePoints.push_back(C);
	}
}



/**************************************************
@brief   : 从文件中读取需要经过的点
@author  : lee
@input   ：文件名(完整路径)/或空 方便调试
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::jsonReader(std::string fileName) {
	if (fileName == "") {
		QString QfileName = QFileDialog::getOpenFileName(NULL,
			QObject::tr("Open json file"),
			QObject::tr("./config"),
			QObject::tr("mesh (*.json);;"
				"All Files (*)"));
		fileName = QfileName.toStdString();
	}
	Json::Value root;
	Json::Reader reader;
	std::ifstream ifs(fileName.c_str());//open file example.json

	if (!reader.parse(ifs, root)) {
		// fail to parse
		std::cout << "fail to parse json \n";
	}
	else {
		// success
		std::cout << "[DEBUG] config file have found!!\n";
		std::cout << "name: " << root["name"].asString() << std::endl;
		p = root["p"].asInt();
		curvePointNum = root["curvePointNum"].asInt();

		Json::Value points = root["points"];
		for (int i = 0; i < points.size(); i++) {
			Point3d p(0, 0, 0);
			p.x = (points[i][0].asDouble());
			p.y = (points[i][1].asDouble());
			p.z = (points[i][2].asDouble());
			Q.push_back(p);
		}
		Json::Value _D0 = root["D0"];
		D0(0) = _D0[0].asDouble();
		D0(1) = _D0[1].asDouble();
		D0(2) = _D0[2].asDouble();
		Json::Value _Dn = root["Dn"];
		Dn(0) = _Dn[0].asDouble();
		Dn(1) = _Dn[1].asDouble();
		Dn(2) = _Dn[2].asDouble();
	}
}


/**************************************************
@brief   : 初始化opengl环境
@author  : lee
@input   ：none
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::initializeGL() {
	glShadeModel(GL_SMOOTH);
	glClearColor(0.1, 0.1, 0.4, 1.0);
	glClearDepth(1.0);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LEQUAL);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
}


/**************************************************
@brief   : 改变窗口的大小
@author  : lee
@input   ：width 宽度
		   height 高度
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::resizeGL(int width, int height)
{
	// 防止height为0
	if (height == 0) {
		height = 1;
	}
	// 重置当前的视口
	glViewport(0, 0, (GLint)width, (GLint)height);
	// 选择投影矩阵
	glMatrixMode(GL_PROJECTION);
	// 重置观察矩阵/投影矩阵 当调用次函数，实际将当前点移到了屏幕中心
	glLoadIdentity();
	// 建立透视投影矩阵,需要<GL/glu.h>头文件
	gluPerspective(45.0, (GLfloat)width / (GLfloat)height, 0.1, 100.0);//角度，长宽比，远近
	// 选择模型观察矩阵
	glMatrixMode(GL_MODELVIEW);
	// 重置观察矩阵/投影矩阵
	glLoadIdentity();
}


/**************************************************
@brief   : 自动和被动 调用更新整个图形
@author  : none
@input   ：none
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::paintGL() {
	//genPoints("normal");
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);// 清除屏幕和深度缓存
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(1.0, -4, -25.0);//平移

	glRotated(rotationX, 1.0, 0.0, 0.0);
	glRotated(rotationY, 0.0, 1.0, 0.0);
	glRotated(rotationZ, 0.0, 0.0, 1.0);//旋转
	glScalef(times, times, times);//缩放
	// 绘制坐标轴
	glBegin(GL_LINE_STRIP);
	glColor3f(0.0, 0.0, 1.0); // 蓝色表示x
	glVertex3d(0, 0, 0);
	glVertex3d(1000, 0, 0);
	glEnd();
	glBegin(GL_LINE_STRIP);
	glColor3f(0.0, 1.0, 0.0);// 绿色  表示 y
	glVertex3d(0, 0, 0);
	glVertex3d(0, 1000, 0);
	glEnd();
	glBegin(GL_LINE_STRIP);
	glColor3f(1.0, 0, 0.0);// 红色表示的是 Z
	glVertex3d(0, 0, 0);
	glVertex3d(0, 0, 1000);
	glEnd();
	// 画控制点
	glPointSize(8.0);
	glBegin(GL_POINTS);
	glColor3f(1.0, 0.0, 0.0);
	for (int i = 0; i < ctlPts.size(); i++) {
		glVertex3d(ctlPts[i].x, ctlPts[i].y, ctlPts[i].z);
	}
	glEnd();

	// 画出控制多边形
	glBegin(GL_LINE_STRIP);
	glColor3f(0.5, 1.0, 0.3);
	for (int i = 0; i < ctlPts.size(); i++) {
		glVertex3d(ctlPts[i].x, ctlPts[i].y, ctlPts[i].z);
	}
	glEnd();

	// 画出要拟合的点
	glBegin(GL_POINTS);
	glColor3f(1.0, 1.0, 0.0);
	for (int i = 0; i < Q.size(); i++) {
		glVertex3d(Q[i].x, Q[i].y, Q[i].z);
	}
	glEnd();

	// 画出曲线
	glBegin(GL_LINE_STRIP);
	glColor3f(1.0, 1.0, 1.0);
	for (int i = 0; i < curvePoints.size(); i++) {
		glVertex3f(curvePoints[i].x, curvePoints[i].y, curvePoints[i].z);
	}
	glEnd();
}



/**************************************************
@brief   : 按键按下事件
@author  : lee
@input   ：e  事件
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::mousePressEvent(QMouseEvent *e) {
	lastPos = e->pos();
	flag = 1;
}


/**************************************************
@brief   : none
@author  : none
@input   ：none
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::mouseReleaseEvent(QMouseEvent *e) {
	flag = -1;
}


/**************************************************
@brief   : 鼠标滚轮事件
@author  : lee
@input   ：event
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::wheelEvent(QWheelEvent *event) {
	if (event->delta() > 0) {// 当滚轮远离使用者时
		times += 0.008f;
		update();
	}
	else {//当滚轮向使用者方向旋转时
		times -= 0.008f;
		update();
	}
}



/**************************************************
@brief   : none
@author  : none
@input   ：none
@output  ：none
@time    : none
**************************************************/
void InterpolateCurveDeri::mouseMoveEvent(QMouseEvent *e) {
	if (flag) {
		GLdouble dx = GLdouble(e->x() - lastPos.x()) / width();//QWidght 的 宽度
		GLdouble dy = GLdouble(e->y() - lastPos.y()) / height();
		if (e->buttons() & Qt::LeftButton) {
			rotationX -= 180 * dy;
			rotationY -= 180 * dx;
			update();
		}
		else if (e->buttons() & Qt::RightButton) {
			rotationX -= 180 * dy;
			rotationZ -= 180 * dx;
			update();
		}
		lastPos = e->pos();
	}
}